#include <DCMotor.h>
#include <CoreLED.h>
#include <CoreKEY.h>
#include <CoreBeep.h>
#include <ExtSRAMInterface.h>
#include <LED.h>
#include <BH1750.h>
#include <Command.h>
#include <BEEP.h>
#include <Infrare.h>
#include <Ultrasonic.h>
#include <SYN7318.h>
#include <Chinese.h>
#include <AgvTask.h>
#include "Drive.h"
/****************************自定义变量**********************************/
uint8_t Go_Speed = 45;    // 全局行进速度值
uint8_t wheel_Speed = 70; // 全局转弯速度值
uint16_t Go_Temp = 300;   // 全局前进码盘值

uint8_t ZigBee_back[16] = {0x55, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};

#define TSendCycle 200
#define ATM_Data_Length 48
uint8_t ATM_Data[ATM_Data_Length];
uint8_t ZigBee_command[8];
uint8_t ZigBee_judge;
uint8_t infrare_com[6];
uint8_t sendflag;
unsigned long frisrtime;
unsigned long Tcount;
unsigned long tim_flag = 0;

uint8_t Data_Type;
uint8_t Data_Flag;
uint8_t Data_Length;
uint8_t Data_OTABuf[40];
uint8_t make;
void ZigBeeRx_Handler(uint8_t *mar);
void OpenMVRx_Handler(uint8_t *mac);
void Analyze_Handle(uint8_t com);
void KEY_Handler(uint8_t k_value);
String DecIntToHexStr(long long num);
void Servo_Control(int8_t angle);

_TASK TASK;
void setup()
{
    CoreLED.Initialization();          // 初始化核心板LED接口
    CoreKEY.Initialization();          // 小车核心板按键初始化
    CoreBeep.Initialization();         // 初始化小车核心板蜂鸣器接口
    ExtSRAMInterface.Initialization(); // 初始化外部总线接口
    LED.Initialization();              // 初始化任务板LED引脚
    BH1750.Initialization();           // 初始化传感器BH1750
    BEEP.Initialization();             // 初始化任务板蜂鸣器引脚
    Infrare.Initialization();          // 红外发射端口初始化
    Ultrasonic.Initialization();       // 初始化超声波模块引脚
    DCMotor.Initialization(72000);     // 直流电机初始化
    SYN7318.Initialization();          // 语音初始化函数
    Serial.begin(115200);              //
    while (!Serial)
        ;

    sendflag = 0;
    frisrtime = 0;
    Tcount = 0;
}

// Serial.print();   // 打印测试
void loop()
{
    uint8_t si = 0;
    frisrtime = millis();
    // AGV_Thread();
    CoreKEY.Kwhile(KEY_Handler); // 按键检测

    if (ExtSRAMInterface.ExMem_Read(0x6100) != 0x00) // 从车接收ZigBee数据
    {
        ExtSRAMInterface.ExMem_Read_Bytes(ZigBee_command, 8);
        ZigBee_judge = ZigBee_command[6]; // 获取校验和
        Command.Judgment(ZigBee_command); // 计算校验和
        if ((ZigBee_judge == ZigBee_command[6]) && (ZigBee_command[0] == 0x55) && (ZigBee_command[7] == 0xBB))
        {
            ZigBeeRx_Handler(ZigBee_command); // ZigBee接收数据处理
        }
    }

    if (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00) // 检测OpenMV识别结果
    {
        Data_Type = ExtSRAMInterface.ExMem_Read(0x603A);
        Data_Flag = ExtSRAMInterface.ExMem_Read(0x603B);
        Data_Length = ExtSRAMInterface.ExMem_Read(0x603C);
        Data_Length = Data_Length + 6;
        ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OTABuf, Data_Length);
        if ((Data_OTABuf[0] == 0x55) && (Data_OTABuf[1] == 0x02))
        {
            ExtSRAMInterface.ExMem_Write_Bytes(0x6080, Data_OTABuf, Data_Length); // 使用自定义数据区上传OpenMV识别结果
            OpenMVRx_Handler(Data_OTABuf);                                        // 接收OpenMV，数据处理函数
        }
    }

    if (((millis() - frisrtime >= TSendCycle) || (Tcount >= TSendCycle)) && (sendflag == 1)) // 获取、上传任务版数据
    {
        uint16_t tp = (uint16_t)(Ultrasonic.Ranging(CM) * 10.0);
        ZigBee_back[5] = (tp >> 8) & 0xff;
        ZigBee_back[4] = tp & 0xff;

        tp = BH1750.ReadLightLevel();
        ZigBee_back[7] = (tp >> 8) & 0xff;
        ZigBee_back[6] = tp & 0xff;

        ZigBee_back[9] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6003);
        if (ZigBee_back[9] >= 0x80)
            ZigBee_back[9] = 0xff - ZigBee_back[9];
        ZigBee_back[8] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6002);

        ExtSRAMInterface.ExMem_Write_Bytes(0x6080, ZigBee_back, 16);
        Tcount = 0x00;
    }
    else if (sendflag == 1)
    {
        Tcount += (millis() - frisrtime);
    }
}
/*
功    能：从车全自动函数
参    数：make
返 回 值：无
*/
void AGV_Thread()
{
    TASK.AGV_Path_Analysis();
}

uint8_t qrdi_buf[8] = {0x55, 0x02, 0x92, 0x00, 0x00, 0x00, 0x00, 0xBB}; // 给OpenMV发送识别二维码
/*
功    能：OpenMV识别二维码启动函数
参    数：无
返 回 值：无
*/
void OpenMVQr_Disc_StartUp(void)
{
    qrdi_buf[3] = 0x01;         // 开始识别
    Command.Judgment(qrdi_buf); // 计算校验和
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);  // 摄像头开始调用识别二维码方法
}
/*
功    能：关闭OpenMV识别二维码函数
参    数：无
返 回 值：无
*/
void OpenMVQr_Disc_CloseUp(void)
{
    qrdi_buf[3] = 0x02;         // 关闭识别
    Command.Judgment(qrdi_buf); // 计算校验和
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);
}

uint8_t servo_buf[8] = {0x55, 0x02, 0x91, 0x03, 0x00, 0x00, 0x00, 0xBB}; // 给OpenMV发送舵机角度
/*
功    能：舵机角度控制函数
参    数：angle: 舵机角度，范围-80至+40，0度垂直于车身
返 回 值：无
*/
void Servo_Control(int8_t angle)
{
    if (angle >= 0)
    {
        servo_buf[4] = 0x2B;
    }
    else
    {
        servo_buf[4] = 0x2D;
    }
    servo_buf[5] = abs(angle);   // 开始识别
    Command.Judgment(servo_buf); // 计算校验和
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, servo_buf, 8);
    delay(1000);
    // Serial.write(servo_buf,8);
}



/*
道闸测试
*/
uint8_t open_road_buf[] = {0x55, 0x03, 0x01, 0x01, 0x00, 0x00, 0x00, 0xBB};
void Road_Gate_Test(void)
{
    Command.Judgment(open_road_buf); // 计算校验和
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, open_road_buf, 8);
}

uint8_t test_buf[] = {0xFD, 0x00, 0x06, 0x01, 0x01, 0xC4, 0xFA, 0xBA, 0xC3};
/*
  语音播报任意内容
*/
void Speech_Sounds_Ctr(void)
{
    ExtSRAMInterface.ExMem_Write_Bytes(0x6180, test_buf, 13);
}

uint8_t repo_buf[] = {0x03, 0x05, 0x14, 0x45, 0xDE, 0x92};
/*
红外测试
*/
void Infrared_Test(void)
{
    Infrare.Transmition(repo_buf, 6);
}

uint8_t trm_buf[] = {0xAF, 0x06, 0x00, 0x02, 0x00, 0x00, 0x01, 0xBB};
/*
语音识别
*/
void Speech_Disc(void)
{
    uint8_t sph_id = 0;
    SYN7318.Start_ASR_send(4);
    delay(200);
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, Command.command33, 8);
    sph_id = SYN7318.Start_ASR_rec(true);
    switch (sph_id)
    {
        Serial.print(SYN7318.Start_ASR_rec(true));
    case 101:
        SYN7318.VSPTest(str[0], 1);
        break;
    case 102:
        SYN7318.VSPTest(str[1], 1);
        break;
    case 103:
        SYN7318.VSPTest(str[2], 1);
        break;
    case 104:
        SYN7318.VSPTest(str[3], 1);
        break;
    case 105:
        SYN7318.VSPTest(str[4], 1);
        break;
    case 106:
        SYN7318.VSPTest(str[5], 1);
        break;
    case 107:
        SYN7318.VSPTest(str[6], 1);
        break;
    default:
        SYN7318.VSPTest(str[12], 1);
        break;
    }
    trm_buf[2] = sph_id;
    ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trm_buf, 8);
}

/**
功   能: 按键处理函数
参   数: K_value 按键值
返回值:无
*/
void KEY_Handler(uint8_t k_value)
{
    switch (k_value)
    {
    case 1:
        BEEP.TurnOn();
        delay(50);
        BEEP.TurnOff();
        Serial.print("启动！\n");
        AGV_Thread(); // 全自动函数
        // make = 0x01;
        break;
    case 2:
        BEEP.TurnOn();
        delay(50);
        BEEP.TurnOff();
        //TASK.Traffic_light_recognition();
        TASK.Send_Data_Using_Zigbee(TASK.K210_trackdi_open);//巡线启动
        break;
    case 3:
        BEEP.TurnOn();
        delay(50);
        BEEP.TurnOff();
        TASK.Send_Data_Using_Zigbee(TASK.K210_trackdi_open);//巡线关闭
        //    BEEP.TurnOn();
        //    delay(50);
        //    BEEP.TurnOff();
        //    TASK.Ultrasonic_Ranging();
        // Speech_Disc();  //语音识别
        break;
    case 4:
        BEEP.TurnOn();
        delay(50);
        BEEP.TurnOff();

        // Road_Gate_Test(); // 打开道闸
        // delay(200);
        // Infrared_Test(); // 红外测试 打开报警器
        // delay(200);
        // Servo_Control(0);
        // delay(200);
        DCMotor.Go(40, 1200);  // 车辆前进
        TASK.QR_code_recognition(); // 识别二维码
        break;
    }
}

/*
功    能：从车ZigBee接收，处理函数
参    数：*mar 接收数据指针
返 回 值：无
*/
void ZigBeeRx_Handler(uint8_t *mar)
{
    switch (mar[1])
    {
    case 0x02:                // 主车
        Analyze_Handler(mar); // 主车命令相应函数
        break;
    case 0x03: // 道闸标志物
        break;
    case 0x04: // LED显示标志物（暂无返回）
        break;
    case 0x05: // 立体车库标志物B
        break;
    case 0x06: // 语音播报标志物
        break;
    case 0x07: // 红外报警标志物
        break;
    case 0x08: // TFT显示标志物B
        break;
    case 0x09: // 调光标志物
        break;
    case 0x0A: // 磁悬浮无线充电标志物
        break;
    case 0x0B: // TFT显示标志物A
        break;
    case 0x0C: // ETC系统标志物
        break;
    case 0x0D: // 立体车库标志物A
        break;
    case 0x0E: // 交通灯标注物A
        break;
    case 0x0F: // 交通灯标志物B
        break;
    default:
        break;
    }
}

/*
功    能：从车接收 机器视觉Camera返回数据
参    数：*mac 接收数据指针
返 回 值：无
*/
void OpenMVRx_Handler(uint8_t *mac)
{
    switch (mac[2])
    {
    case 0x91: // 保留

        break;
    case 0x92: // 二维码识别

        break;
    default:
        break;
    }
}

/**
功    能：从车任务处理函数
参    数：com 主指令
返 回 值：无
*/
void Analyze_Handler(uint8_t *com)
{
    switch (com[2])
    {
    case 0x01: // 停止
        DCMotor.Stop();
        break;
    case 0x02: // 前进
        DCMotor.Go(com[3], (com[4] + (com[5] << 8)));
        ZigBee_back[2] = 0x03;
        break;
    case 0x03: // 后退
        DCMotor.Back(com[3], (com[4] + (com[5] << 8)));
        ZigBee_back[2] = 0x03;
        break;
    case 0x04: // 左转
        DCMotor.TurnLeft(com[3]);
        ZigBee_back[2] = 0x02;
        break;
    case 0x05: // 右转
        DCMotor.TurnRight(com[3]);
        ZigBee_back[2] = 0x02;
        break;
    case 0x06: // 循迹
        DCMotor.CarTrack(com[3]);
        ZigBee_back[2] = 0x00;
        break;
    case 0x07:                                                    // 码盘清零
        Command.Judgment(Command.command01);                      // 计算校验和
        ExtSRAMInterface.ExMem_Write_Bytes(Command.command01, 8); // 码盘清零
        break;
    case 0x10: // 保存红外数据
        infrare_com[0] = com[3];
        infrare_com[1] = com[4];
        infrare_com[2] = com[5];
        break;
    case 0x11: // 保存红外数据
        infrare_com[3] = com[3];
        infrare_com[4] = com[4];
        infrare_com[5] = com[5];
        break;
    case 0x12: // 发送红外数据
        Infrare.Transmition(infrare_com, 6);
        break;
    case 0x20: // 左右转向灯
        if (com[3] == 0x01)
            LED.LeftTurnOn();
        else
            LED.LeftTurnOff();
        if (com[4] == 0x01)
            LED.RightTurnOn();
        else
            LED.RightTurnOff();
        break;
    case 0x30: // 蜂鸣器
        if (com[3] == 0x01)
            BEEP.TurnOn();
        else
            BEEP.TurnOff();
        break;
    case 0x40: // 保留
        break;
    case 0x50: // LCD图片上翻页
        if (com[3] == 0x01)
        {
            Command.Judgment(Command.command13);
            ExtSRAMInterface.ExMem_Write_Bytes(Command.command13, 8);
        }
        else if (com[3] == 0x02)
        {
            Command.Judgment(Command.command13);
            ExtSRAMInterface.ExMem_Write_Bytes(Command.command34, 8);
        }
        break;
    case 0x51: // LCD图片下翻页
        if (com[3] == 0x01)
        {
            Command.Judgment(Command.command14);
            ExtSRAMInterface.ExMem_Write_Bytes(Command.command14, 8);
        }
        else if (com[3] == 0x02)
        {
            Command.Judgment(Command.command13);
            ExtSRAMInterface.ExMem_Write_Bytes(Command.command35, 8);
        }
        break;
    case 0x61: // 光源加一档
        Infrare.Transmition(Command.HW_Dimming1, 4);
        break;
    case 0x62: // 光源加二挡
        Infrare.Transmition(Command.HW_Dimming2, 4);
        break;
    case 0x63: // 光源加三挡
        Infrare.Transmition(Command.HW_Dimming3, 4);
        break;
    case 0x80: // 从车返回数据切换
        if (com[3] == 0x01)
        {
            sendflag = 0x01; // 上传从车数据
        }
        else
        {
            sendflag = 0x00; // 关闭上从车数据
        }
        break;
    case 0x92:
        if (com[3] == 0x01)
        {
            OpenMVQr_Disc_StartUp(); // 识别二维码
        }
        else
        {
            OpenMVQr_Disc_CloseUp(); // 关闭识别二维码
        }
        break;
    // case 0x93:
    //     if (com[3] == 0x01)
    //     {
    //         OpenMVQr_Disc_StartUp(); // 识别交通灯
    //     }
    //     else
    //     {
    //         OpenMVQr_Disc_CloseUp(); // 关闭识别交通灯
    //     }
    //     break;
    case 0xA0:
    {
        BEEP.TurnOn();
        delay(100);
        BEEP.TurnOff();
        AGV_Thread();
    }
    break;
    default:
        break;
    }
}
